#!/usr/bin/env roseus
;;(unix:putenv "ROS_MASTER_URI=http://pr1012:11311")
(ros::load-ros-manifest "jsk_pr2_gui")
(ros::roseus "test_nav")
(require "package://pr2eus/pr2-interface.l")

(defvar *tfl* (instance ros::transform-listener :init))
(defvar *tfb* (instance ros::transform-broadcaster :init))

(defun nav-init-setting()
  (setq move-base-action (instance ros::simple-action-client :init
				   "move_base" move_base_msgs::MoveBaseAction))
  (load-temp-scene)
;;  (setq *spots-list* (list "/temp/f/place3" "/temp/f/place2" "/temp/f/place1" "/temp/f/place0" "/temp/f/place3"))
  (setq *spots-list* (list "/temp/f/place7" "/temp/f/place6" "/temp/f/place5" "/temp/f/place4" "/temp/f/place3" "/temp/f/place2" "/temp/f/place1" "/temp/f/place0" "/temp/f/place11" "/temp/f/place00"))
  (ros::spin-once)
  )

(defun load-temp-scene ()
  (load "package://jsk_maps/src/eng2-scene.l")
  (load "package://jsk_maps/src/temp-scene.l")
  (setq *temp-scene* (make-temp-scene)))

;; (move-to-spot "/temp/f/place2")
;; (progn (move-to-spot "/temp/f/place2") (move-to-spot "/temp/f/place1") (move-to-spot "/temp/f/place0"))

(defun move-to-spot-sequence (spot-list)
  (dolist (i *spots-list*)
    (move-to-spot i)
    (ros::ros-warn "move-to-sequence: next spots")))

(defun move-to-spot (spot &key (frame-id "/world"))
  (let* ((s2 (send *temp-scene* :find-object spot)))
    (cond ((null s2)
           (ros::ros-warn "move-to-spot: no such place like ~A" spot)
           (return-from move-to-spot nil))
          (t
           (print spot)
	   (move-to-feedback-test (car s2))))
    ;;    (speak-jp "とうちゃく")
    ))

(defun move-to-feedback-test ;; *human-detected* nil
  (cds &key (retry 10) (frame-id "/world") (wait-for-server-timeout 5))
  (let (ret (is_goaled nil) (is_reached nil) (count 0) (tm (ros::time-now))
	    (pose (ros::coords->tf-pose cds))
	    (goal (instance move_base_msgs::MoveBaseActionGoal :init)))
    (when (not (send move-base-action :wait-for-server wait-for-server-timeout))
      (ros::ros-error "move base action not found")
      (return-from :move-to nil))

     (send goal :header :stamp tm)
     (send goal :goal :target_pose :header :stamp tm)
     (send goal :goal :target_pose :header :frame_id frame-id)
     (send goal :goal :target_pose :pose pose)

     (send move-base-action :send-goal goal)
     (setq map-goal-coords (send (send *tfl* :lookup-transform "/map" frame-id (ros::time 0)) :transform (send cds :copy-worldcoords)))

     (while (and (null is_goaled) (null is_reached)) ;; (<= (inc count) retry))
       ;; (if (> count 0)
       ;;    (clear-costmap))
       (ros::ros-info "move-to : send-goal to ~A (~d)" (send cds :worldpos) count)
       (ros::sleep) ;; (send move-base-action :wait-for-result)
       (if (eq (send move-base-action :get-state) actionlib_msgs::GoalStatus::*succeeded*)
	   (setq is_goaled t))
       (ros::spin-once)
       (ros::sleep)
       (ros::spin-once)
       (ros::sleep)
       (if *human-detected*
	   (progn (ros::ros-warn "move-to-feedback: human detected")
		  (unsubscribe-human)
		  (send move-base-action :cancel-all-goals)
		  (setq *human-detected* nil)
		  (body-callback *human-msg*)
		  (wait-for-hand-over)
		  (setq *time-count* (ros::time-now))
		  (unix:sleep 2)
		  (subscribe-human)
		  (return-from move-to-feedback-test nil)
		  ))
       (let (diff current-coords)
	 (when (null (send *tfl* :wait-for-transform "/map" "/base_footprint" (ros::time-now) 5))
	   (ros::ros-error ":move-to wait-for transform /map to /base_footprint failed")
   (return-from :move-to-feedback-test nil))
	 (setq current-coords (send *tfl* :lookup-transform "/map" "/base_footprint" (ros::time 0)))
	 (setq diff (send current-coords :transformation map-goal-coords))
	 (if (< (norm (subseq (send diff :worldpos) 0 2)) 500)
	   (progn (ros::ros-warn "move-to-feedback: less than 200mm")
		  (setq is_reached t))
	   (ros::ros-debug "move-to-feedback: more than 200mm")))
       )
     ))

;; (move-to-spot-sequennce *spots-list*)